/*
 * @Author: wangke
 * @Date: 2025-03-13 22:44:05
 * @LastEditors: wangke
 * @LastEditTime: 2025-05-26 17:00:45
 * 
 */
#include "BspOid.h"
#include "Arduino.h"
#include "Common.h"
#include "PIN.h"



/* Private typedef -----------------------------------------------------------*/

typedef enum
{
    OID_OLD64BIT = BSP_OID_OLD64BIT,    /**< Oid old 64bit format */
    OID_OLD96BIT = BSP_OID_OLD96BIT,    /**< Oid old 96bit format */
    OID_NEW96BIT,                       /**< Oid new 96bit format */
} OID_ALL_FORMAT_E;

typedef enum
{
    OID_STA_UNINIT = 0,     /**< Oid need init */
    OID_STA_NORMAL,         /**< Oid is normal */
    OID_STA_SLEEP,          /**< Oid is sleep */
} OID_STATUS_E;

typedef struct
{
    OID_ALL_FORMAT_E eFmtNow;       /**< Oid current format information */
    BSP_OID_FORMAT_E eOldFmt;       /**< Oid old format type information */
    BSP_OID_FORMAT_E eStartUpFmt;   /**< Oid start up format information */
    OID_STATUS_E     eStatus;       /**< Oid in sleep or not */
} Oid_Info_t;


/* Private define ------------------------------------------------------------*/

#define OID_STARTUP_SCK_HIGHTIME ( 30 ) /**< Delay ms cnt for sck high to weak up oid */
#define OID_STARTUP_TIMEOUT_CNT ( 300 ) /**< Time out ms cnt for weak up oid */
#define OID_SENDCMD_TIMEOUT_CNT ( 150 ) /**< Time out ms cnt for send oid cmd */
/* Private macro -------------------------------------------------------------*/

#define prv2WIREIO_SCKHIGH_DELAY()  delayMicroseconds( 4 )        /**< Delay us cnt for sck high in oid 2wire interface, at least 4us */
#define prv2WIREIO_SCKLOW_DELAY()   delayMicroseconds( 2 )        /**< Delay us cnt for sck low in oid 2wire interface, at least 2us */
#define prv2WIREIO_SCKSTOP_DELAY()  delayMicroseconds( 80 )       /**< Delay us cnt for sck low to stop 2wire, at least 78us */
#define prvOID_DELAYMS( X )         delay( X )         /**< Delay ms function for oid */
#define prvOID_DELAYUS( X )         delayMicroseconds( X )        /**< Delay us function for oid */

#define prvDISABLE_IRQ()            ;     /**< Disable interrupt function for oid */
#define prvENABLE_IRQ()             ;//( __enable_irq() )      /**< Enable interrupt function for oid */

#define prvOID_CLRWATCHDOG()        ;//WDT_Clear( WDT_ID_0 )   /**< Clear watch dog */

/* Private variables ---------------------------------------------------------*/
static Oid_Info_t s_tOidInfo;
void prvOidIo_Init()
{
    pinMode(BSP_OID_SCKIO,OUTPUT );
    pinMode(BSP_OID_SDIOIO,INPUT );
}
/**
  * @brief  Sck io operate.
  */
static void prv2WireIo_SckHigh( void )
{
    // Set sck io high
    digitalWrite(BSP_OID_SCKIO, HIGH);
}

static void prv2WireIo_SckLow( void )
{
    // Set sck io low
    digitalWrite( BSP_OID_SCKIO , LOW );
}

/**
  * @brief  Sdio io operate.
  */
 static void prv2WireIo_SetSdioOutput( void )
 {
     // Set sdio io output
     pinMode(BSP_OID_SDIOIO,OUTPUT );
 }

 static void prv2WireIo_SdioHigh( void )
{
    // Set sdio io high
    digitalWrite( BSP_OID_SDIOIO , HIGH );
}

static void prv2WireIo_SdioLow( void )
{
    // Set sdio io low
    digitalWrite( BSP_OID_SDIOIO , LOW );
}

static void prv2WireIo_SetSdioInput( void )
{
    // Set sdio io input
	pinMode( BSP_OID_SDIOIO ,INPUT );
}

static bool prv2WireIo_GetSdioStatus( void )
{
	// Get data in sdio
    return digitalRead( BSP_OID_SDIOIO );
}

/**
  * @brief  Check if there is data need to receive.
  * @retval 0: Idle; Other: Need receive data
  */
 static bool prv2Wire_CheckRx( void )
 {
    return ( prv2WireIo_GetSdioStatus() == 0 ) ? true : false;
 }

 /**
  * @brief  Receive flow of 2wire interface.
  * @param  uint32_t* pulHigh32Bits: pointer to save high 32 bit data
  *         uint32_t* pulMiddle32Bits: pointer to save middle 32 bit data (unused for old 64bit)
  *         uint32_t* pulLow32Bits: pointer to save low 32 bit data
  */
static void prv2Wire_RxData( uint32_t* pulHigh32Bits , uint32_t * pulMiddle32Bits , uint32_t * pulLow32Bits )
{
    uint16_t i;
    uint16_t usBitLength;
	uint32_t ulTemp32BitData = 0;
    
    // Reset data
	*pulHigh32Bits = 0;
	*pulMiddle32Bits = 0;
	*pulLow32Bits = 0;
    
    // Set bit length
	if( s_tOidInfo.eFmtNow == OID_OLD64BIT )
	{
		usBitLength = 64;
	}
	else
	{
		usBitLength = 96;
	}
    
    // Set sck high to start a communication and set sdio 0 to send a read bit
	prv2WireIo_SckHigh();
	prv2WIREIO_SCKHIGH_DELAY();
	prv2WIREIO_SCKHIGH_DELAY();
	prv2WireIo_SetSdioOutput();
	prv2WireIo_SdioLow();
	prv2WIREIO_SCKHIGH_DELAY();
    
    // Oid get bit when sck low
	prvDISABLE_IRQ();
	prv2WireIo_SckLow();
	prv2WIREIO_SCKLOW_DELAY();
	prv2WireIo_SckHigh();
	prvENABLE_IRQ();
    
    // Set sdio input to get bits
	prv2WireIo_SetSdioInput();
    for( i = 0; i < usBitLength; i++ )
    {
        // Sck high for oid to put bits
		prv2WireIo_SckHigh();
		prvENABLE_IRQ();
		prv2WIREIO_SCKHIGH_DELAY();
		prvDISABLE_IRQ();
		prv2WireIo_SckLow();
        
        // Get bits when sck low
		ulTemp32BitData <<= 1;
		if( prv2WireIo_GetSdioStatus() != 0 )
		{
			ulTemp32BitData += 1;
		}
        
        // Save bits
		if( i == 31 )
		{
			*pulHigh32Bits = ulTemp32BitData;
			ulTemp32BitData = 0;
		}
		else if( i == 63)
		{
			if( usBitLength == 64 )
			{
				*pulLow32Bits = ulTemp32BitData;
			}
			else
			{
				*pulMiddle32Bits = ulTemp32BitData;
			}
      
			ulTemp32BitData = 0;
		}
		else if( i == 95 )
		{
			*pulLow32Bits = ulTemp32BitData;
			ulTemp32BitData = 0;
		}
    
		prv2WIREIO_SCKLOW_DELAY();
    }
    
    // Sck low at least 78us to stop
	prvENABLE_IRQ();
	prv2WireIo_SckLow();
	prv2WIREIO_SCKSTOP_DELAY();
}

/**
  * @brief  Sned command to oid with 2wire interface.
  * @param  uint8_t ucCmd: cmd to send
  * @retval 0: Ok; Other: Fail
  */
 static int32_t prv2Wire_SendCmd( uint8_t ucCmd )
 {
     uint16_t i;
     uint16_t usTemp;
     uint32_t ulHigh32Bits;
     uint32_t ulMiddle32Bits;
     uint32_t ulLow32Bits;
 
     // Check need rx first
     if( prv2WireIo_GetSdioStatus() == 0 )
     {
         prv2Wire_RxData( &ulHigh32Bits , &ulMiddle32Bits , &ulLow32Bits );
     }
     
     // Sck high to start a communication and set sdio 1 to send a write bit
      prv2WireIo_SckHigh();
     prv2WIREIO_SCKHIGH_DELAY();
     prv2WIREIO_SCKHIGH_DELAY();
     prv2WireIo_SetSdioOutput();
     prv2WireIo_SdioHigh();
     prv2WIREIO_SCKHIGH_DELAY();
     
     // Oid get write bit when sck low
     prvDISABLE_IRQ();
     prv2WireIo_SckLow();
     prv2WIREIO_SCKLOW_DELAY();
     
     // Send cmd bits
     usTemp = ucCmd;
     for( i = 0 ; i < 8 ; i++ )
     {
         // Put bits when sck high
         prv2WireIo_SckHigh();
         prvENABLE_IRQ();
         if( ( usTemp & 0x80 ) == 0 )
         {
             prv2WireIo_SdioLow();
         }
         else
         {
             prv2WireIo_SdioHigh();
         }
     
         // Oid get bits when sck low
         prv2WIREIO_SCKHIGH_DELAY();
         prvDISABLE_IRQ();
         prv2WireIo_SckLow();
         prv2WIREIO_SCKLOW_DELAY();
         usTemp <<= 1;
     }
   
     // Sck low at least 78us to stop
     prv2WireIo_SetSdioInput();
     prvENABLE_IRQ();
     prv2WireIo_SckLow();
     prv2WIREIO_SCKSTOP_DELAY();
 
     // Polling to rx ack
     for( i = 0; i < OID_SENDCMD_TIMEOUT_CNT; i++ )
     {
         prvOID_CLRWATCHDOG();
         if( prv2WireIo_GetSdioStatus() == 0 )
         {
             // Need to rx ack
             break;
         }
 
         prvOID_DELAYMS( 1 );
     }
 
     // Check polling time out
     if( i >= OID_SENDCMD_TIMEOUT_CNT )
     {
         return SONIX_APP_ERROR_TIMEOUT;
     }
     
     // Set sck high to start a communication and set sdio 0 to send a read bit
     prv2WireIo_SckHigh();
     prv2WIREIO_SCKHIGH_DELAY();
     prv2WIREIO_SCKHIGH_DELAY();
     prv2WireIo_SetSdioOutput();
     prv2WireIo_SdioLow();
     prv2WIREIO_SCKHIGH_DELAY();
     
     // Oid get bit when sck low
     prvDISABLE_IRQ();
     prv2WireIo_SckLow();
     prv2WIREIO_SCKLOW_DELAY();
     prv2WireIo_SckHigh();
     prvENABLE_IRQ();
     
     // Set sdio input to get bits
     prv2WireIo_SetSdioInput();
     usTemp = 0;
     for( i = 0 ; i < 16 ; i++ )
     {
         // Sck high for oid to put bits
         prv2WireIo_SckHigh();
         prvENABLE_IRQ();
         prv2WIREIO_SCKHIGH_DELAY();
         prvDISABLE_IRQ();
         prv2WireIo_SckLow();
         
         // Get bits when sck low
         usTemp <<= 1;
         if( prv2WireIo_GetSdioStatus() != 0 )
         {
             usTemp += 1;
          }
     
         prv2WIREIO_SCKLOW_DELAY();
     }
   
     // Sck low at least 78us to stop
     prvENABLE_IRQ();
     prv2WireIo_SckLow();
     prv2WIREIO_SCKSTOP_DELAY();
 
     // Check whether ack = cmd + 1
     ucCmd = ucCmd + 1;
     if( ucCmd == ( uint8_t ) usTemp )
     {
         return SONIX_APP_OK;
     }
     else
     {
         return SONIX_APP_ERROR_INVALID;
     }
 }

/**
  * @brief  Read register value with 2 wire interface.
  * @param  uint16_t usAddr: register address
  *         uint16_t* pusData: pointer to save data
  * @retval 0: Ok; Other: Fail
  */
 static int32_t prv2Wire_ReadReg( uint16_t usAddr , uint16_t* pusData )
 {
     int32_t iRes;
     uint16_t i;
     uint32_t ulHigh32Bits;
     uint32_t ulMiddle32Bits;
     uint32_t ulLow32Bits;
     uint8_t ucaCmdBuffer[ 5 ];
     
     /*
        A[0] : Cmd 0x74 is Read Decoder Reg Cmd;
        A[1] : Num of Parameter : 3;
        A[2] : Parameter 1 : High Byte of Addr;
        A[3] : Parameter 2 : Low Byte of Addr;
        A[4] : Parameter 3 : Checksum of A[1]~A[3];
     */
     ucaCmdBuffer[ 0 ] = ( uint8_t ) 0x74;
     ucaCmdBuffer[ 1 ] = ( uint8_t ) 3;
     ucaCmdBuffer[ 2 ] = ( uint8_t ) ( usAddr >> 8 );
     ucaCmdBuffer[ 3 ] = ( uint8_t ) ( usAddr & 0x00FF );
     ucaCmdBuffer[ 4 ] = ucaCmdBuffer[ 1 ] + ucaCmdBuffer[ 2 ] + ucaCmdBuffer[ 3 ];
     
     // Send commands
     for( i = 0 ; i < 5 ; i++ )
     {
         prvOID_CLRWATCHDOG();
         iRes = prv2Wire_SendCmd( ucaCmdBuffer[ i ] );
         if( iRes != SONIX_APP_OK )
         {
             return iRes;
         }
     
         prvOID_DELAYMS( 2 );
     }
 
     // Polling to rx ack
     for( i = 0; i < OID_SENDCMD_TIMEOUT_CNT; i++ )
     {
         prvOID_CLRWATCHDOG();
         if( prv2WireIo_GetSdioStatus() == 0 )
         {
             // Need to rx ack
             break;
         }
 
         prvOID_DELAYMS( 1 );
     }
 
     // Check polling time out
     if( i >= OID_SENDCMD_TIMEOUT_CNT )
     {
         return SONIX_APP_ERROR_TIMEOUT;
     }
     
     // Read ack and get data
     prv2Wire_RxData( &ulHigh32Bits , &ulMiddle32Bits , &ulLow32Bits );
     if( ( ulHigh32Bits & 0xE0000000 ) == 0xE0000000 )
     {
         if( ( s_tOidInfo.eFmtNow == OID_OLD64BIT ) || ( s_tOidInfo.eFmtNow == OID_OLD96BIT ) )
         {
             // Old 64bit return : 0xe0000000, 0xaaaavvvv( aaaa : regaddr , vvvv : regvalue );
             // Old 96bit return : 0xe0000000, 0x00000000, 0xaaaavvvv( aaaa : regaddr , vvvv : regvalue );
             if( ( ( ulLow32Bits >> 16 ) & 0x0000FFFF ) == ( uint32_t ) usAddr )
             {
                 *pusData = ( uint16_t ) ( ulLow32Bits & 0x0000FFFF );
                 return SONIX_APP_OK;
             }
         }
         else
         {
             // New 96bit return : 0xe000aaaa( aaaa : regaddr ), 0xvvvv0000( vvvv : regvalue ), 0x00000000;
             if( ( ulHigh32Bits & 0x0000FFFF ) == ( uint32_t ) usAddr )
             {
                 *pusData = ( uint16_t ) ( ulMiddle32Bits >> 16 ) & 0x0000FFFF;
                 return SONIX_APP_OK;
             }
         }
     }
     
     return SONIX_APP_ERROR_INVALID;
 }

/**
  * @brief  Write register with 2 wire interface.
  * @param  uint16_t usAddr: register address
  *         uint16_t usData: data to write
  * @retval 0: Ok; Other: Fail
  */
 static int32_t prv2Wire_WriteReg( uint16_t usAddr , uint16_t usData )
 {
     int32_t iRes;
     uint16_t i;
     uint8_t ucaCmdBuffer[ 7 ];
     
     /*
        A[0] : Cmd 0x73 is Write Decoder Reg Cmd;
        A[1] : Num of Parameter : 5;
        A[2] : Parameter 1 : High Byte of Addr;
        A[3] : Parameter 2 : Low Byte of Addr;
        A[4] : Parameter 3 : High Byte of Value;
        A[5] : Parameter 4 : Low Byte of Value;
        A[6] : Parameter 5 : Checksum of A[1]~A[5];
     */
     ucaCmdBuffer[ 0 ] = ( uint8_t ) 0x73;
     ucaCmdBuffer[ 1 ] = ( uint8_t ) 5;
     ucaCmdBuffer[ 2 ] = ( uint8_t ) ( usAddr >> 8 );
     ucaCmdBuffer[ 3 ] = ( uint8_t ) ( usAddr & 0x00FF );
     ucaCmdBuffer[ 4 ] = ( uint8_t ) ( usData >> 8 );
     ucaCmdBuffer[ 5 ] = ( uint8_t ) ( usData & 0x00FF );
     ucaCmdBuffer[ 6 ] = ucaCmdBuffer[ 1 ] + ucaCmdBuffer[ 2 ] + ucaCmdBuffer[ 3 ] + ucaCmdBuffer[ 4 ] + ucaCmdBuffer[ 5 ];
   
     // Send commands
     for( i = 0 ; i < 7 ; i++ )
     {
         prvOID_CLRWATCHDOG();
         iRes = prv2Wire_SendCmd( ucaCmdBuffer[ i ] );
         if( iRes != SONIX_APP_OK )
         {
             return iRes;
         }
     
         prvOID_DELAYMS( 2 );
     }
     
     return SONIX_APP_OK;
 }

// =========================== Oid Parse Function Implement ================= //
/**
  * @brief  Parse oid raw data bit in 64 old format.
  * @param  Bsp_Oid_Data_t* ptOidData: pointer to save parsed res
  *         uint32_t ulHigh32Bits: high 32 bits of raw data
  *         uint32_t ulLow32Bits: low 32 bits of raw data
  * @retval BSP_OID_TYPE_E
  */
 static BSP_OID_TYPE_E prvOidParse_Old64Bit( Bsp_Oid_Data_t* ptOidData , uint32_t ulHigh32Bits , uint32_t ulLow32Bits )
 {
     uint32_t ulTypeTemp;
     uint32_t ulPosInteger;
     uint32_t ulPosDecimal;
     BSP_OID_TYPE_E eDataType;
     
     // Bit63 = 1, cmd ack
     if( ( ulHigh32Bits & 0x80000000 ) == 0x80000000 )
     {
         if( ptOidData != 0 )
         {
             // Bit[15:0] is cmd ack
             ptOidData->eType   = BSP_OID_TYPE_CMD;
             ptOidData->ulData1 = ulLow32Bits & 0x0000FFFF;
             ptOidData->ulData2 = 0;
             ptOidData->ulAngle = 0;
         }
     
         return BSP_OID_TYPE_CMD;
     }
     
     // Bit63 = 0, Index
     ulTypeTemp = ulHigh32Bits & 0xF0000000;
     switch( ulTypeTemp )
     {
         case 0x00000000 :
         {
             // Bit[63:60] = 0000(b) : oid2 general
             eDataType = BSP_OID_TYPE_2GEN;
             if( ptOidData != 0 )
             {
                 // Bit[53:45] is angle, bit[15:0] is index
                 ptOidData->eType   = BSP_OID_TYPE_2GEN;
                 ptOidData->ulData1 = ulLow32Bits & 0x0000FFFF;
                 ptOidData->ulData2 = 0;
                 ptOidData->ulAngle = ( ulHigh32Bits >> 13 ) & 0x000001FF;
             }
     
             break;
         }
 
         case 0x40000000:
         {
             // Bit[63:60] = 0100(b) : oid3/oid3s general
             eDataType = BSP_OID_TYPE_OLDGEN;
             if( ptOidData != 0 )
             {
                 // Bit[53:45] is angle, bit[27:0] is index
                 ptOidData->eType   = BSP_OID_TYPE_OLDGEN;
                 ptOidData->ulData1 = ulLow32Bits & 0x0FFFFFFF;
                 ptOidData->ulData2 = 0;
                 ptOidData->ulAngle = ( ulHigh32Bits >> 13 ) & 0x000001FF;
             }
 
             break;
         }
 
         case 0x60000000:
         {
             // Bit[63:60] = 0110(b) : oid3/oid3s position
             eDataType = BSP_OID_TYPE_OLDPOS;
             if( ptOidData != 0 )
             {
                 // X position : integer:bit[13:0], decimal:bit[35:28]
                 ulPosInteger = ulLow32Bits & 0x00003FFF;
                 ulPosDecimal = ulHigh32Bits & 0x0000000F;
                 ulPosDecimal <<= 4;
                 ulPosDecimal += ( ( ulLow32Bits >> 28 ) & 0x0000000F );
                 if( ( ulPosDecimal & 0x00000080 ) == 0x00000080 )
                 {
                     ulPosDecimal = ( ( ~ ulPosDecimal ) & 0x00000007F ) + 1;
                     ptOidData->ulData1 = ( ulPosInteger << 7 ) - ulPosDecimal;
                 }
                 else
                 {
                     ptOidData->ulData1 = ( ulPosInteger << 7 ) + ulPosDecimal;
                 }
 
                 // Y position : integer:bit[27:14], decimal:bit[43:36]
                 ulPosInteger = ( ulLow32Bits >> 14 ) & 0x00003FFF;
                 ulPosDecimal = ( ulHigh32Bits >> 4 ) & 0x000000FF;
                 if( ( ulPosDecimal & 0x00000080 ) == 0x00000080 )
                 {
                     ulPosDecimal = ( ( ~ ulPosDecimal ) & 0x00000007F ) + 1;
                     ptOidData->ulData2 = ( ulPosInteger << 7 ) - ulPosDecimal;
                 }
                 else
                 {
                     ptOidData->ulData2 = ( ulPosInteger << 7 ) + ulPosDecimal;
                 }
 
                 // Bit[53:45] is angle
                 ptOidData->ulAngle = ( ulHigh32Bits >> 13 ) & 0x000001FF;
                 ptOidData->eType = eDataType;
             }
 
             break;
         }
 
         default:
         {
             // Bit[63:60] = other value, invalid index
             ulTypeTemp = ulHigh32Bits & 0x1C000000;
             switch( ulTypeTemp )
             {
                 case 0x10000000:
                 {
                     // Bit63 = 0, bit60 = 1, bit[59:58] = 00(b) : invalid as decode fail
                     eDataType = BSP_OID_TYPE_DECFAIL;
                     if( ptOidData != 0 )
                     {
                         ptOidData->eType   = BSP_OID_TYPE_DECFAIL;
                         ptOidData->ulData1 = 0;
                         ptOidData->ulData2 = 0;
                         ptOidData->ulAngle = 0;
                     }
 
                     break;
                 }
 
                 case 0x18000000:
                 {
                     // Bit63 = 0, bit60 = 1, bit[59:58] = 10(b) : invalid as less dot
                     eDataType = BSP_OID_TYPE_LESSDOT;
                     if( ptOidData != 0 )
                     {
                         ptOidData->eType   = BSP_OID_TYPE_LESSDOT;
                         ptOidData->ulData1 = 0;
                         ptOidData->ulData2 = 0;
                         ptOidData->ulAngle = 0;
                     }
 
                     break;
                 }
 
                 case 0x1C000000:
                 {
                     // Bit63 = 0, bit60 = 1, bit[59:58] = 11(b) : invalid as angle only
                     eDataType = BSP_OID_TYPE_ANGLEONLY;
                     if( ptOidData != 0 )
                     {
                         // Bit[57:49] is angle
                         ptOidData->eType   = BSP_OID_TYPE_ANGLEONLY;
                         ptOidData->ulData1 = 0;
                         ptOidData->ulData2 = 0;
                         ptOidData->ulAngle = ( ulHigh32Bits >> 17) & 0x000001FF;
                     }
 
                     break;
                 }
 
                 default:
                 {
                     // Unkonw
                     eDataType = BSP_OID_TYPE_UNKNOW;
                     if( ptOidData != 0 )
                     {
                         ptOidData->eType   = BSP_OID_TYPE_UNKNOW;
                         ptOidData->ulData1 = 0;
                         ptOidData->ulData2 = 0;
                         ptOidData->ulAngle = 0;
                     }
 
                     break;
                 }
             }
 
             break;
         }
     }
 
     return eDataType;
 }
/**
  * @brief  Parse oid raw data bit in 96 old format.
  * @param  Bsp_Oid_Data_t* ptOidData: pointer to save parsed res
  *         uint32_t ulHigh32Bits: high 32 bits of raw data
  *         uint32_t ulMiddle32Bits: middle 32 bits of raw data
  *         uint32_t ulLow32Bits: low 32 bits of raw data
  * @retval BSP_OID_TYPE_E
  */
 static BSP_OID_TYPE_E prvOidParse_Old96Bit( Bsp_Oid_Data_t* ptOidData , uint32_t ulHigh32Bits , uint32_t ulMiddle32Bits , uint32_t ulLow32Bits )
 {
     uint32_t ulTypeTemp;
     uint32_t ulPosInteger;
     uint32_t ulPosDecimal;
     BSP_OID_TYPE_E eDataType;
     
     // Bit95 = 1, cmd ack
     if( ( ulHigh32Bits & 0x80000000 ) == 0x80000000 )
     {
         if( ptOidData != 0 )
         {
             // Bit[15:0] is cmd ack
             ptOidData->eType   = BSP_OID_TYPE_CMD;
             ptOidData->ulData1 = ulLow32Bits & 0x0000FFFF;
             ptOidData->ulData2 = 0;
             ptOidData->ulAngle = 0;
         }
     
         return BSP_OID_TYPE_CMD;
     }
     
     // Bit95 = 0, Index
     ulTypeTemp = ulHigh32Bits & 0xF0000000;
     switch( ulTypeTemp )
     {
         case 0x00000000 :
         {
             // Bit[95:92] = 0000(b) : oid2 general
             eDataType = BSP_OID_TYPE_2GEN;
             if( ptOidData != 0 )
             {
                 // Bit[85:77] is angle, bit[47:32] is index
                 ptOidData->eType   = BSP_OID_TYPE_2GEN;
                 ptOidData->ulData1 = ulMiddle32Bits & 0x0000FFFF;
                 ptOidData->ulData2 = 0;
                 ptOidData->ulAngle = ( ulHigh32Bits >> 13 ) & 0x000001FF;
             }
     
             break;
         }
 
         case 0x40000000:
         {
             // Bit[95:92] = 0100(b) : oid3/oid3s/oid4 general
             if( ulLow32Bits == 0 )
             {
                 eDataType = prvOidParse_Old64Bit( ptOidData , ulHigh32Bits , ulMiddle32Bits );
                 break;
             }
             
             eDataType = BSP_OID_TYPE_4GEN;
             if( ptOidData != 0 )
             {
                 // Bit[85:77] is angle, bit[45:0] is index
                 ptOidData->eType   = BSP_OID_TYPE_4GEN;
                 ptOidData->ulData1 = ulLow32Bits;
                 ptOidData->ulData2 = ulMiddle32Bits & 0x00003FFF;
                 ptOidData->ulAngle = ( ulHigh32Bits >> 13 ) & 0x000001FF;
             }
 
             break;
         }
 
         case 0x60000000:
         {
             // Bit[95:92] = 0110(b) : oid3/oid3s/oid4 position
             if( ulLow32Bits == 0 )
             {
                 eDataType = prvOidParse_Old64Bit( ptOidData , ulHigh32Bits , ulMiddle32Bits );
                 break;
             }
             
             eDataType = BSP_OID_TYPE_4POS;
             if( ptOidData != 0 )
             {
                 // X position : integer:bit[22:0], decimal:bit[53:46]
                 ulPosInteger = ulLow32Bits & 0x007FFFFF;
                 ulPosDecimal = ( ulMiddle32Bits >> 14 ) & 0x000000FF;
                 if( ( ulPosDecimal & 0x00000080 ) == 0x00000080 )
                 {
                     ulPosDecimal = ( ( ~ ulPosDecimal ) & 0x00000007F ) + 1;
                     ptOidData->ulData1 = ( ulPosInteger << 7 ) - ulPosDecimal;
                 }
                 else
                 {
                     ptOidData->ulData1 = ( ulPosInteger << 7 ) + ulPosDecimal;
                 }
 
                 // Y position : integer:bit[45:23],decimal:bit[61:54]
                 ulPosInteger = ulMiddle32Bits & 0x00003FFF;
                 ulPosInteger <<= 9;
                 ulPosInteger += ( ulLow32Bits >> 23 ) & 0x000001FF;
                 ulPosDecimal = ( ulMiddle32Bits >> 22 ) & 0x000000FF;
                 if( ( ulPosDecimal & 0x00000080 ) == 0x00000080 )
                 {
                     ulPosDecimal = ( ( ~ ulPosDecimal ) & 0x00000007F ) + 1;
                     ptOidData->ulData2 = ( ulPosInteger << 7 ) - ulPosDecimal;
                 }
                 else
                 {
                     ptOidData->ulData2 = ( ulPosInteger << 7 ) + ulPosDecimal;
                 }
 
                 // Bit[85:77] is angle
                 ptOidData->ulAngle = ( ulHigh32Bits >> 13 ) & 0x000001FF;
                 ptOidData->eType = eDataType;
             }
 
             break;
         }
 
         default:
         {
             // Bit[95:92] = other value, invalid index
             ulTypeTemp = ulHigh32Bits & 0x1C000000;
             switch( ulTypeTemp )
             {
                 case 0x10000000:
                 {
                     // Bit95 = 0, bit92 = 1, bit[91:90] = 00(b) : invalid as decode fail
                     eDataType = BSP_OID_TYPE_DECFAIL;
                     if( ptOidData != 0 )
                     {
                         ptOidData->eType   = BSP_OID_TYPE_DECFAIL;
                         ptOidData->ulData1 = 0;
                         ptOidData->ulData2 = 0;
                         ptOidData->ulAngle = 0;
                     }
 
                     break;
                 }
 
                 case 0x18000000:
                 {
                     // Bit95 = 0, bit92 = 1, bit[91:90] = 10(b) : invalid as less dot
                     eDataType = BSP_OID_TYPE_LESSDOT;
                     if( ptOidData != 0 )
                     {
                         ptOidData->eType   = BSP_OID_TYPE_LESSDOT;
                         ptOidData->ulData1 = 0;
                         ptOidData->ulData2 = 0;
                         ptOidData->ulAngle = 0;
                     }
 
                     break;
                 }
 
                 case 0x1C000000:
                 {
                     // Bit95 = 0, bit92 = 1, bit[91:90] = 11(b) : invalid as angle only
                     eDataType = BSP_OID_TYPE_ANGLEONLY;
                     if( ptOidData != 0 )
                     {
                         // Bit[89:81] is angle
                         ptOidData->eType   = BSP_OID_TYPE_ANGLEONLY;
                         ptOidData->ulData1 = 0;
                         ptOidData->ulData2 = 0;
                         ptOidData->ulAngle = ( ulHigh32Bits >> 17) & 0x000001FF;
                     }
 
                     break;
                 }
 
                 default:
                 {
                     // Unkonw
                     eDataType = BSP_OID_TYPE_UNKNOW;
                     if( ptOidData != 0 )
                     {
                         ptOidData->eType   = BSP_OID_TYPE_UNKNOW;
                         ptOidData->ulData1 = 0;
                         ptOidData->ulData2 = 0;
                         ptOidData->ulAngle = 0;
                     }
 
                     break;
                 }
             }
 
             break;
         }
     }
 
     return eDataType;
 }
/**
  * @brief  Parse oid raw data bit in 96 new format.
  * @param  Bsp_Oid_Data_t* ptOidData: pointer to save parsed res
  *         uint32_t ulHigh32Bits: high 32 bits of raw data
  *         uint32_t ulMiddle32Bits: middle 32 bits of raw data
  *         uint32_t ulLow32Bits: low 32 bits of raw data
  * @retval BSP_OID_TYPE_E
  */
 static BSP_OID_TYPE_E prvOidParse_New96Bit( Bsp_Oid_Data_t* ptOidData , uint32_t ulHigh32Bits , uint32_t ulMiddle32Bits , uint32_t ulLow32Bits )
 {
     uint16_t i;
     uint32_t ulChkSum;
     uint32_t ulChkSumTemp;
     uint32_t ulDataTemp;
     uint32_t ulTypeTemp;
     uint32_t ulPosInteger;
     uint32_t ulPosDecimal;
     BSP_OID_TYPE_E eDataType;  
   
     // Bit95 = 1, cmd ack
     if( ( ulHigh32Bits & 0x80000000 ) == 0x80000000 )
     {
         if( ptOidData != 0 )
         {
             // Bit[79:64] is cmd ack
             ptOidData->eType   = BSP_OID_TYPE_CMD;
             ptOidData->ulData1 = ulHigh32Bits & 0x0000FFFF;
             ptOidData->ulData2 = 0;
             ptOidData->ulAngle = 0;
         }
     
         return BSP_OID_TYPE_CMD;
     }
     
     // Bit95 = 0, Index
     ulTypeTemp = ulHigh32Bits & 0xFC000000;
     switch( ulTypeTemp )
     {
         case 0x00000000 :
         {
             // Bit[95:90] = 000000(b) : oid2 general
             eDataType = BSP_OID_TYPE_2GEN;
             if( ptOidData != 0 )
             {
                 // Bit[73:65] is angle, bit[89:74] is index
                 ptOidData->eType   = eDataType;
                 ptOidData->ulData1 = ( ulHigh32Bits >> 10 ) & 0x0000FFFF;
                 ptOidData->ulData2 = 0;
                 ptOidData->ulAngle = ( ulHigh32Bits >> 1 ) & 0x000001FF;
       
                 // Compute checksum
                 ulDataTemp = ulHigh32Bits;
                 ulChkSumTemp = ulDataTemp & 0x0000000E;
                 for( i = 0 ; i < 7 ; i++ )
                 {
                     ulDataTemp >>= 4;
                     ulChkSumTemp += ulDataTemp & 0x0000000F;
                 }
       
                 // Compare checksum
                 ulChkSumTemp &= 0x0000000F;
                 ulChkSum = ulHigh32Bits & 0x00000001;
                 ulChkSum <<= 3;
                 ulChkSum += ( ulMiddle32Bits >> 29 ) & 0x00000007;
                 if( ulChkSumTemp != ulChkSum )
                 {
                     // Checksum error
                     eDataType = BSP_OID_TYPE_CHKSUMERR;
                 }
             }
     
             break;
         }
       
         case 0x08000000 :
         case 0x10000000 :
         {
             // Bit[95:90] = 000010(b) : oid3 general; bit[95:90] = 000100(b) : oid3s general
             if( ulTypeTemp == 0x08000000 )
             {
                 eDataType = BSP_OID_TYPE_3GEN;
             }
             else
             {
                 eDataType = BSP_OID_TYPE_3SGEN;
             }
     
             // Oid3 general and oid3s general have the same format
             if( ptOidData != 0 )
             {
                 // Bit[89:62] is index
                 ulDataTemp = ulHigh32Bits & 0x03FFFFFF;
                 ulDataTemp <<= 2;
                 ulDataTemp += ( ulMiddle32Bits >> 30 ) & 0x00000003;
       
                 // Bit[61:53] is angle
                 ptOidData->eType   = eDataType;
                 ptOidData->ulData1 = ulDataTemp;
                 ptOidData->ulData2 = 0;
                 ptOidData->ulAngle = ( ulMiddle32Bits >> 21 ) & 0x000001FF;
       
                 // Compute checksum
                 ulChkSumTemp = ( ulMiddle32Bits >> 20 ) & 0x0000000E;
                 ulChkSumTemp += ( ulMiddle32Bits >> 24 ) & 0x0000000F;
                 ulChkSumTemp += ( ulMiddle32Bits >> 28 ) & 0x0000000F;
                 ulDataTemp = ulHigh32Bits;
                 for( i = 0 ; i < 8 ; i++ )
                 {
                     ulChkSumTemp += ulDataTemp & 0x0000000F;
                     ulDataTemp >>= 4;
                 }
       
                 // Compare checksum
                 ulChkSumTemp &= 0x0000000F;
                 ulChkSum = ( ulMiddle32Bits >> 17 ) & 0x0000000F;
                 if( ulChkSumTemp != ulChkSum )
                 {
                     // Checksum error
                     eDataType = BSP_OID_TYPE_CHKSUMERR;
                 }
             }
     
             break;
         }
       
         case 0x04000000 :
         case 0x0C000000 :
         {
             // Bit[95:90] = 000001(b) : oid3 position; bit[95:90] = 000011(b) : oid3s position
             if( ulTypeTemp == 0x04000000 )
             {
                 eDataType = BSP_OID_TYPE_3POS;
             }
             else
             {
                 eDataType = BSP_OID_TYPE_3SPOS;
             }
     
             // Oid3 position and oid3s position have the same format
             if( ptOidData != 0 )
             {
                 // X position : integer:bit[75:62], decimal:bit[53:46]
                 ulPosInteger = ulHigh32Bits & 0x00000FFF;
                 ulPosInteger <<= 2;
                 ulPosInteger += ( ulMiddle32Bits >> 30 ) & 0x00000003;
                 ulPosDecimal = ( ulMiddle32Bits >> 14 ) & 0x000000FF;
                 if( ( ulPosDecimal & 0x00000080 ) == 0x00000080 )
                 {
                     ulPosDecimal = ( ( ~ ulPosDecimal ) & 0x00000007F ) + 1;
                     ptOidData->ulData1 = ( ulPosInteger << 7 ) - ulPosDecimal;
                 }
                 else
                 {
                     ptOidData->ulData1 = ( ulPosInteger << 7 ) + ulPosDecimal;
                 }
       
                 // Y position : integer:bit[89:76], decimal:bit[61:54]
                 ulPosInteger = ( ulHigh32Bits >> 12 ) & 0x00003FFF;
                 ulPosDecimal = ( ulMiddle32Bits >> 22 ) & 0x000000FF;
                 if( ( ulPosDecimal & 0x00000080 ) == 0x00000080 )
                 {
                     ulPosDecimal = ( ( ~ ulPosDecimal ) & 0x00000007F ) + 1;
                     ptOidData->ulData2 = ( ulPosInteger << 7 ) - ulPosDecimal;
                 }
                 else
                 {
                     ptOidData->ulData2 = ( ulPosInteger << 7 ) + ulPosDecimal;
                 }
       
                 // Bit[45:37] is angle
                 ptOidData->ulAngle = ( ulMiddle32Bits >> 5 ) & 0x000001FF;
                 ptOidData->eType   = eDataType;
       
                 // Compute checksum
                 ulChkSumTemp = ( ulMiddle32Bits >> 4 ) & 0x0000000E;
                 ulDataTemp = ulMiddle32Bits >> 4;
                 for( i = 0 ; i < 6 ; i++ )
                 {
                     ulDataTemp >>= 4;
                     ulChkSumTemp += ulDataTemp & 0x0000000F;
                 }
       
                 ulDataTemp = ulHigh32Bits;
                 for( i = 0 ; i < 8 ; i++ )
                 {
                     ulChkSumTemp += ulDataTemp & 0x0000000F;
                     ulDataTemp >>= 4;
                 }
       
                 // Compare checksum
                 ulChkSumTemp &= 0x0000000F;
                 ulChkSum = ( ulMiddle32Bits >> 1 ) & 0x0000000F;
                 if( ulChkSumTemp != ulChkSum )
                 {
                     // Checksum error
                     eDataType = BSP_OID_TYPE_CHKSUMERR;
                 }
             }
     
             break;
         }
       
         case 0x14000000 :
         {
             // Bit[95:90] = 000101(b) : oid3.5 general
             eDataType = BSP_OID_TYPE_3P5GEN;
             if( ptOidData != 0 )
             {
                 // Bit[89:56] is index
                 ulDataTemp = ulHigh32Bits & 0x00FFFFFF;
                 ulDataTemp <<= 8;
                 ulDataTemp += ( ulMiddle32Bits >> 24 ) & 0x000000FF;
       
                 // Bit[55:47] is angle
                 ptOidData->eType   = eDataType;
                 ptOidData->ulData1 = ulDataTemp;
                 ptOidData->ulData2 = ( ulHigh32Bits >> 24 ) & 0x00000003;
                 ptOidData->ulAngle = ( ulMiddle32Bits >> 15 ) & 0x000001FF;
       
                 // Compute checksum
                 ulChkSumTemp = ( ulMiddle32Bits >> 12 ) & 0x00000008;
                 ulDataTemp = ulMiddle32Bits >> 12;
                 for( i = 0 ; i < 4 ; i++ )
                 {
                     ulDataTemp >>= 4;
                     ulChkSumTemp += ulDataTemp & 0x0000000F;
                 }
       
                 ulDataTemp = ulHigh32Bits;
                 for( i = 0 ; i < 8 ; i++ )
                 {
                     ulChkSumTemp += ulDataTemp & 0x0000000F;
                     ulDataTemp >>= 4;
                 }
       
                 // Compare checksum
                 ulChkSumTemp &= 0x0000000F;
                 ulChkSum = ( ulMiddle32Bits >> 11 ) & 0x0000000F;
                 if( ulChkSumTemp != ulChkSum )
                 {
                     // Checksum error
                     eDataType = BSP_OID_TYPE_CHKSUMERR;
                 }
             }
     
             break;
         }
       
         case 0x1C000000 :
         {
             // Bit[95:90] = 000111(b) : oid4 general
             eDataType = BSP_OID_TYPE_4GEN;
             if( ptOidData != 0 )
             {
                 // Bit[89:44] is index
                 ulDataTemp = ulHigh32Bits & 0x00000FFF;
                 ulDataTemp <<= 20;
                 ulDataTemp += ( ulMiddle32Bits >> 12 ) & 0x000FFFFF;
       
                 // Bit[43:35] is angle
                 ptOidData->eType   = eDataType;
                 ptOidData->ulData1 = ulDataTemp;
                 ptOidData->ulData2 = ( ulHigh32Bits >> 12 ) & 0x00003FFF;
                 ptOidData->ulAngle = ( ulMiddle32Bits >> 3 ) & 0x000001FF;
       
                 // Compute checksum
                 ulDataTemp = ulMiddle32Bits & 0xFFFFFFF8;
                 ulChkSumTemp = 0;
                 for( i = 0 ; i < 8 ; i++ )
                 {
                     ulChkSumTemp += ulDataTemp & 0x0000000F;
                     ulDataTemp >>= 4;
                 }
       
                 ulDataTemp = ulHigh32Bits;
                 for( i = 0 ; i < 8 ; i++ )
                 {
                     ulChkSumTemp += ulDataTemp & 0x0000000F;
                     ulDataTemp >>= 4;
                 }
       
                 // Compare checksum
                 ulChkSumTemp &= 0x0000000F;
                 ulChkSum = ulMiddle32Bits & 0x00000007;
                 ulChkSum <<= 1;
                 ulChkSum += ulLow32Bits >> 31;
                 if( ulChkSumTemp != ulChkSum )
                 {
                     // Checksum error
                     eDataType = BSP_OID_TYPE_CHKSUMERR;
                 }
             }
     
             break;
         }
       
         case 0x18000000 :
         {
             // Bit[95:90] = 000110(b) : oid4 position
             eDataType = BSP_OID_TYPE_4POS;
             if( ptOidData != 0 )
             {
                 // X position : integer:bit[66:44], decimal:bit[35:28]
                 ulPosInteger = ulHigh32Bits & 0x00000007;
                 ulPosInteger <<= 20;
                 ulPosInteger += ( ulMiddle32Bits >> 12 ) & 0x000FFFFF;
                 ulPosDecimal = ulMiddle32Bits & 0x0000000F;
                 ulPosDecimal <<= 4;
                 ulPosDecimal += ( ulLow32Bits >> 28 ) & 0x0000000F;
                 if( ( ulPosDecimal & 0x00000080 ) == 0x00000080 )
                 {
                     ulPosDecimal = ( ( ~ ulPosDecimal ) & 0x00000007F ) + 1;
                     ptOidData->ulData1 = ( ulPosInteger << 7 ) - ulPosDecimal;
                 }
                 else
                 {
                     ptOidData->ulData1 = ( ulPosInteger << 7 ) + ulPosDecimal;
                 }
       
                 // Y position : integer:bit[89:67], decimal:bit[43:36]
                 ulPosInteger = ( ulHigh32Bits >> 3 ) & 0x007FFFFF;
                 ulPosDecimal = ( ulMiddle32Bits >> 4 ) & 0x000000FF;
                 if( ( ulPosDecimal & 0x00000080 ) == 0x00000080 )
                 {
                     ulPosDecimal = ( ( ~ ulPosDecimal ) & 0x00000007F ) + 1;
                     ptOidData->ulData2 = ( ulPosInteger << 7 ) - ulPosDecimal;
                 }
                 else
                 {
                     ptOidData->ulData2 = ( ulPosInteger << 7 ) + ulPosDecimal;
                 }
       
                 // Bit[27:19] is angle
                 ptOidData->ulAngle = ( ulLow32Bits >> 19 ) & 0x000001FF;
                 ptOidData->eType   = eDataType;
       
                 // Compute checksum
                 ulChkSumTemp = ( ulLow32Bits >> 16 ) & 0x00000008;
                 ulChkSumTemp += ( ulLow32Bits >> 20 ) & 0x0000000F;
                 ulChkSumTemp += ( ulLow32Bits >> 24 ) & 0x0000000F;
                 ulChkSumTemp += ( ulLow32Bits >> 28 ) & 0x0000000F;
       
                 ulDataTemp = ulMiddle32Bits;
                 for( i = 0 ; i < 8 ; i++ )
                 {
                     ulChkSumTemp += ulDataTemp & 0x0000000F;
                     ulDataTemp >>= 4;
                 }
       
                 ulDataTemp = ulHigh32Bits;
                 for( i = 0 ; i < 8 ; i++ )
                 {
                     ulChkSumTemp += ulDataTemp & 0x0000000F;
                     ulDataTemp >>= 4;
                 }
       
                 // Compare checksum
                 ulChkSumTemp &= 0x0000000F;
                 ulChkSum = ( ulLow32Bits >> 15 ) & 0x0000000F;
                 if( ulChkSumTemp != ulChkSum )
                 {
                     // Checksum error
                     eDataType = BSP_OID_TYPE_CHKSUMERR;
                 }
             }
     
             break;
         }
       
         case 0x20000000 :
         {
             // Bit[95:90] = 001000(b) : oid4c position
             eDataType = BSP_OID_TYPE_4C;
             if( ptOidData != 0 )
             {
                 // X position : integer:bit[68:48], decimal:bit[39:32]
                 ulPosInteger = ulHigh32Bits & 0x0000001F;
                 ulPosInteger <<= 16;
                 ulPosInteger += ( ulMiddle32Bits >> 16 ) & 0x0000FFFF;
                 ulPosDecimal = ulMiddle32Bits & 0x000000FF;
                 if( ( ulPosDecimal & 0x00000080 ) == 0x00000080 )
                 {
                     ulPosDecimal = ( ( ~ ulPosDecimal ) & 0x00000007F ) + 1;
                     ptOidData->ulData1 = ( ulPosInteger << 7 ) - ulPosDecimal;
                 }
                 else
                 {
                     ptOidData->ulData1 = ( ulPosInteger << 7 ) + ulPosDecimal;
                 }
       
                 // Y position : integer:bit[89:69], decimal:bit[47:40]
                 ulPosInteger = ( ulHigh32Bits >> 5 ) & 0x001FFFFF;
                 ulPosDecimal = ( ulMiddle32Bits >> 8 ) & 0x000000FF;
                 if( ( ulPosDecimal & 0x00000080 ) == 0x00000080 )
                 {
                     ulPosDecimal = ( ( ~ ulPosDecimal ) & 0x00000007F ) + 1;
                     ptOidData->ulData2 = ( ulPosInteger << 7 ) - ulPosDecimal;
                 }
                 else
                 {
                     ptOidData->ulData2 = ( ulPosInteger << 7 ) + ulPosDecimal;
                 }
       
                 // Bit[31:23] is angle
                 ptOidData->ulAngle = ( ulLow32Bits >> 23 ) & 0x000001FF;
                 ptOidData->eType   = eDataType;
       
                 // Compute checksum
                 ulChkSumTemp = ( ulLow32Bits >> 20 ) & 0x00000008;
                 ulChkSumTemp += ( ulLow32Bits >> 24 ) & 0x0000000F;
                 ulChkSumTemp += ( ulLow32Bits >> 28 ) & 0x0000000F;
       
                 ulDataTemp = ulMiddle32Bits;
                 for( i = 0 ; i < 8 ; i++ )
                 {
                     ulChkSumTemp += ulDataTemp & 0x0000000F;
                     ulDataTemp >>= 4;
                 }
       
                 ulDataTemp = ulHigh32Bits;
                 for( i = 0 ; i < 8 ; i++ )
                 {
                     ulChkSumTemp += ulDataTemp & 0x0000000F;
                     ulDataTemp >>= 4;
                 }
       
                 // Compare checksum
                 ulChkSumTemp &= 0x0000000F;
                 ulChkSum = ( ulLow32Bits >> 19 ) & 0x0000000F;
                 if( ulChkSumTemp != ulChkSum )
                 {
                     // Checksum error
                     eDataType = BSP_OID_TYPE_CHKSUMERR;
                 }
             }
     
             break;
         }
       
         default :
         {
             // Bit[95:90] = other value, invalid index
             ulTypeTemp = ulHigh32Bits & 0xF0000000;
             switch( ulTypeTemp )
             {
                 case 0x40000000 :
                 {
                     // Bit[95:92] = 0100(b) : invalid as decode fail
                     eDataType = BSP_OID_TYPE_DECFAIL;
                     if( ptOidData != 0 )
                     {
                         ptOidData->eType   = eDataType;
                         ptOidData->ulData1 = 0;
                         ptOidData->ulData2 = 0;
                         ptOidData->ulAngle = 0;
           
                         // Check checksum
                         ulChkSum = ( ulHigh32Bits << 4 ) & 0xF0000000;
                         if( ulChkSum != ulTypeTemp )
                         {
                             // Checksum error
                             eDataType = BSP_OID_TYPE_CHKSUMERR;
                         }
                     }
         
                     break;
                 }
       
                 case 0x60000000 :
                 {
                     // Bit[95:92] = 0110(b) : invalid as less dot
                     eDataType = BSP_OID_TYPE_LESSDOT;
                     if( ptOidData != 0 )
                     {
                         ptOidData->eType   = eDataType;
                         ptOidData->ulData1 = 0;
                         ptOidData->ulData2 = 0;
                         ptOidData->ulAngle = 0;
           
                         // Check checksum
                         ulChkSum = ( ulHigh32Bits << 4 ) & 0xF0000000;
                         if( ulChkSum != ulTypeTemp )
                         {
                             // Checksum error
                             eDataType = BSP_OID_TYPE_CHKSUMERR;
                         }
                     }
         
                     break;
                 }
       
                 case 0x70000000 :
                 {
                     // Bit[95:92] = 0111(b) : invalid as angle only
                     eDataType = BSP_OID_TYPE_ANGLEONLY;
                     if( ptOidData != 0 )
                     {
                         // Bit[91:83] is angle
                         ptOidData->eType   = eDataType;
                         ptOidData->ulData1 = 0;
                         ptOidData->ulData2 = 0;
                         ptOidData->ulAngle = ( ulHigh32Bits >> 19 ) & 0x000001FF;
           
                         // Compute checksum
                         ulChkSumTemp = ( ulHigh32Bits >> 16 ) & 0x00000008;
                         ulChkSumTemp += ( ulHigh32Bits >> 20 ) & 0x0000000F;
                         ulChkSumTemp += ( ulHigh32Bits >> 24 ) & 0x0000000F;
                         ulChkSumTemp += ( ulHigh32Bits >> 28 ) & 0x0000000F;
           
                         // Compare checksum
                         ulChkSumTemp &= 0x0000000F;
                         ulChkSum = ( ulHigh32Bits >> 15 ) & 0x0000000F;
                         if( ulChkSumTemp != ulChkSum )
                         {
                             // Checksum error
                             eDataType = BSP_OID_TYPE_CHKSUMERR;
                         }
                     }
         
                     break;
                 }
       
                 default :
                 {
                     // Unkonw
                     eDataType = BSP_OID_TYPE_UNKNOW;
                     if( ptOidData != 0 )
                     {
                         ptOidData->eType   = eDataType;
                         ptOidData->ulData1 = 0;
                         ptOidData->ulData2 = 0;
                         ptOidData->ulAngle = 0;
                     }
         
                     break;
                 }
             }
     
             break;
         }
     }
 
     return eDataType;
 }
 
 /* Exported user code --------------------------------------------------------*/
 
 /**
   * @brief  Initialize oid resource.
   */
 void Bsp_Oid_Init( void )
 {
     prvOidIo_Init();
     
     s_tOidInfo.eFmtNow     = OID_OLD64BIT;
     s_tOidInfo.eOldFmt     = BSP_OID_OLD64BIT;
     s_tOidInfo.eStartUpFmt = BSP_OID_OLD64BIT;
     s_tOidInfo.eStatus     = OID_STA_UNINIT;
 }
 
 #if BSP_OID_USE_NRST
 
 /**
   * @brief  Makes oid enter reset status.
   */
 void Bsp_Oid_EnterReset( void )
 {
     prvOidIo_NRstLow();
 }
 
 /**
   * @brief  Makes oid leave reset status.
   */
 void Bsp_Oid_LeaveReset( void )
 {
     prvOidIo_NRstHigh();
 }
 
 #endif
 
 /**
   * @brief  The oid startup flow.
   * @param  BSP_OID_FORMAT_E eStartUpFmt: oid start up data format, refer to spec
   * @retval 0: Ok; Other: Fail
   */
 int32_t Bsp_Oid_StartUp( BSP_OID_FORMAT_E eStartUpFmt )
 {
     uint16_t i;
     uint32_t ulHigh32Bits;
     uint32_t ulMiddle32Bits;
     uint32_t ulLow32Bits;
     
     // Check status
     if( s_tOidInfo.eStatus != OID_STA_UNINIT )
     {
         return SONIX_APP_ERROR;
     }
     
     // Change oid information
     s_tOidInfo.eStatus     = OID_STA_NORMAL;
     s_tOidInfo.eFmtNow     = ( OID_ALL_FORMAT_E ) eStartUpFmt;
     s_tOidInfo.eOldFmt     = eStartUpFmt;
     s_tOidInfo.eStartUpFmt = eStartUpFmt;
     
     // Sck high for at least 20ms to weak up oid
     prv2WireIo_SckHigh();
     prvOID_DELAYMS( OID_STARTUP_SCK_HIGHTIME );
     prv2WireIo_SckLow();
     
     // Polling rx ack
     for( i = 0; i < OID_STARTUP_TIMEOUT_CNT; i++ )
     {
         prvOID_CLRWATCHDOG();
         if( prv2WireIo_GetSdioStatus() == 0 )
         {
             // Need to rx ack
             break;
         }
 
         prvOID_DELAYMS( 1 );
     }
 
     // Check polling time out
     if( i >= OID_STARTUP_TIMEOUT_CNT )
     {
         return SONIX_APP_ERROR_TIMEOUT;
     }
     
     // Rx ack and check valid
     prv2Wire_RxData( &ulHigh32Bits , &ulMiddle32Bits , &ulLow32Bits );
     if( ( ulHigh32Bits & 0x80000000 ) == 0x80000000 )
     {
         if( ( ulLow32Bits & 0x0000FFFF ) == 0x0000FFF8 )
         {
             return SONIX_APP_OK;
         }
     }
 
     return SONIX_APP_ERROR_INVALID;
 }
 
 /**
   * @brief  Check if need to receive data or not.
   * @retval 0: Idle; Other: Need to receive
   */
 bool Bsp_Oid_CheckRx( void )
 {
     // Check status
     if( s_tOidInfo.eStatus != OID_STA_NORMAL )
     {
         return false;
     }
     
     return prv2Wire_CheckRx();
 }
 
 /**
   * @brief  Receive oid data.
   * @param  Bsp_Oid_Data_t* ptOidData: pointer to save oid data
   * @retval 0: Ok; Other: Fail
   */
 int32_t Bsp_Oid_GetData( Bsp_Oid_Data_t* ptOidData )
 {
     uint32_t ulHigh32Bits;
     uint32_t ulMiddle32Bits;
     uint32_t ulLow32Bits;
     
     // Check status
     if( s_tOidInfo.eStatus != OID_STA_NORMAL )
     {
        //printfme("Bsp_Oid_GetData : sonix app error\r\n");
         return SONIX_APP_ERROR;
     }
     
     // Rx data and parse it
     memset( ptOidData , 0 , sizeof( Bsp_Oid_Data_t ) );
     if( prv2Wire_CheckRx() != false )
     {
         prv2Wire_RxData( &ulHigh32Bits , &ulMiddle32Bits , &ulLow32Bits );
         if( s_tOidInfo.eFmtNow == OID_OLD64BIT )
         {
             // Parse old 64bit
             prvOidParse_Old64Bit( ptOidData , ulHigh32Bits , ulLow32Bits );
         }
         else if( s_tOidInfo.eFmtNow == OID_OLD96BIT )
         {
             // Parse old 96bit
             prvOidParse_Old96Bit( ptOidData , ulHigh32Bits , ulMiddle32Bits , ulLow32Bits );
         }
         else if( s_tOidInfo.eFmtNow == OID_NEW96BIT )
         {
             // Parse new 96bit
             if( prvOidParse_New96Bit( ptOidData , ulHigh32Bits , ulMiddle32Bits , ulLow32Bits ) == BSP_OID_TYPE_CHKSUMERR )
             {
                //printfme("sonix app error invalid\r\n");
                 return SONIX_APP_ERROR_INVALID;
             }
         }
         
         return SONIX_APP_OK;
     }
     printfme("sonix app check rx fail\r\n");
     return SONIX_APP_ERROR_NOTRDY;
 }
 
 /**
   * @brief  Sned cmd to oid device.
   * @param  uint8_t ucCmd: command to send
   * @retval 0: Ok; Other: Fail
   */
 int32_t Bsp_Oid_SendCmd( uint8_t ucCmd )
 {
     int32_t iRes;
     
     // Check status
     if( s_tOidInfo.eStatus != OID_STA_NORMAL )
     {
         return SONIX_APP_ERROR;
     }
     
     // Send cmd and update oid information
     iRes = prv2Wire_SendCmd( ucCmd );
     if( iRes == SONIX_APP_OK )
     {
         if( ( ucCmd == 0x2C ) || ( ucCmd == 0x2E ) )
         {
             // Set new format
             s_tOidInfo.eFmtNow = OID_NEW96BIT;
         }
         else if( ( ucCmd == 0x28 ) || ( ucCmd == 0x2A ) )
         {
             // Return old format
             s_tOidInfo.eFmtNow = ( OID_ALL_FORMAT_E ) s_tOidInfo.eOldFmt;
         }
         else if( ucCmd == 0x57 )
         {
             // Enter sleep
             s_tOidInfo.eStatus = OID_STA_SLEEP;
         }
     }
     
     return iRes;
 }
 
 /**
   * @brief  Read register value of oid device.
   * @param  uint16_t usAddr: register address
   *         uint16_t* pusData: pointer to save data
   * @retval 0: Ok; Other: Fail
   */
 int32_t Bsp_Oid_ReadReg( uint16_t usAddr , uint16_t* pusData )
 {
     // Check status
     if( s_tOidInfo.eStatus != OID_STA_NORMAL )
     {
         return SONIX_APP_ERROR;
     }
     
     return prv2Wire_ReadReg( usAddr , pusData );
 }
 
 /**
   * @brief  Write data to register of oid device.
   * @param  uint16_t usAddr: register address
   *         uint16_t usData: data to write
   * @retval 0: Ok; Other: Fail
   */
 int32_t Bsp_Oid_WriteReg( uint16_t usAddr , uint16_t usData )
 {
     int32_t iRes;
     
     // Check status
     if( s_tOidInfo.eStatus != OID_STA_NORMAL )
     {
         return SONIX_APP_ERROR;
     }
     
     // Write register and update oid information
     iRes = prv2Wire_WriteReg( usAddr , usData );
     if( ( iRes == SONIX_APP_OK ) && ( usAddr == 0x0D40 ) )
     {
         if( ( usData == 0x0000 ) || ( usData == 0x0010 ) )
         {
             // Only oid3/oid3s , return old 64bit format
             s_tOidInfo.eFmtNow = OID_OLD64BIT;
             s_tOidInfo.eOldFmt = BSP_OID_OLD64BIT;
         }
         else if( usData == 0x0020 )
         {
             // Only oid4 , return old 96bit format
             s_tOidInfo.eFmtNow = OID_OLD96BIT;
             s_tOidInfo.eOldFmt = BSP_OID_OLD96BIT;
         }
         else if( usData == 0x0028 )
         {
             // Disable only , return start up format
             s_tOidInfo.eFmtNow = ( OID_ALL_FORMAT_E ) s_tOidInfo.eStartUpFmt;
             s_tOidInfo.eOldFmt = s_tOidInfo.eStartUpFmt;
         }
     }
     
     return iRes;
 }
 
 /**
   * @brief  Make oid device enter sleep mode.
   * @retval 0: Ok; Other: Fail
   */
 int32_t Bsp_Oid_EnterSleep( void )
 {
     return Bsp_Oid_SendCmd( 0x57 );
 }
 
 /**
   * @brief  Weak up oid device from sleep mode.
   * @retval 0: Ok; Other: Fail
   */
 int32_t Bsp_Oid_LeaveSleep( void )
 {
     // Check status
     if( s_tOidInfo.eStatus != OID_STA_SLEEP )
     {
         return SONIX_APP_OK;
     }
     
     // Weak up oid by set sck high for at least 2us
     prv2WireIo_SckHigh();
     prvOID_DELAYUS( 3 );
     prv2WireIo_SckLow();
     s_tOidInfo.eStatus = OID_STA_NORMAL;
     return SONIX_APP_OK;
 }
 
 /**
   * @brief  Get rom version of oid device.
   * @param  BSP_OID_VER_E* peRomVer: pointer to save version
   * @retval 0: Ok; Other: Fail
   */
 int32_t Bsp_Oid_GetRomVer( BSP_OID_VER_E* peRomVer )
 {
     int32_t iRes;
     uint16_t usData;
     
     // Check status
     if( s_tOidInfo.eStatus != OID_STA_NORMAL )
     {
         return SONIX_APP_ERROR;
     }
     
     // Read reg 0x0E1F
     prvOID_DELAYMS( 2 );
     iRes = prv2Wire_ReadReg( 0x0E1F , &usData );
     if( iRes != SONIX_APP_OK )
     {
         return iRes;
     }
     
     // Reg(0x0E1F) != 0x0210, rom011
     if( usData == 0x0210 )
     {
         *peRomVer = BSP_OID_VER_ROM011;
         return SONIX_APP_OK;
     }
     
     // Read reg 0x1670
     prvOID_DELAYMS( 2 );
     iRes = prv2Wire_ReadReg( 0x1670 , &usData );
     if( iRes != SONIX_APP_OK )
     {
         return iRes;
     }
     
     // Reg(0x1670) != 0x1168, rom 003
     if( usData != 0x1168 )
     {
         *peRomVer = BSP_OID_VER_ROM003;
         return SONIX_APP_OK;
     }
     
     // Read reg 0x0DA6
     prvOID_DELAYMS( 2 );
     iRes = prv2Wire_ReadReg( 0x0DA6 , &usData );
     if( iRes != SONIX_APP_OK )
     {
         return iRes;
     }
     
     // Reg(0x0DA6) == 9, rom 009
     if( usData == 9 )
     {
         *peRomVer = BSP_OID_VER_ROM009;
         return SONIX_APP_OK;
     }
     
     // Reg(0x0DA6) == 0, rom 008
     if( usData == 0 )
     {
         *peRomVer = BSP_OID_VER_ROM008;
         return SONIX_APP_OK;
     }
     
     // Read reg 0x0D40
     prvOID_DELAYMS( 2 );
     iRes = prv2Wire_ReadReg( 0x0D40 , &usData );
     if( iRes != SONIX_APP_OK )
     {
         return iRes;
     }
     
     // Reg(0x0D40) != 0, rom 006
     if( usData != 0 )
     {
         *peRomVer = BSP_OID_VER_ROM006;
         return SONIX_APP_OK;
     }
     
     // Read reg 0x0DF8
     prvOID_DELAYMS( 2 );
     iRes = prv2Wire_ReadReg( 0x0DF8 , &usData );
     if( iRes != SONIX_APP_OK )
     {
         return iRes;
     }
     
     // Reg(0x0DF8) == 7, rom 005
     if( usData == 7 )
     {
         *peRomVer = BSP_OID_VER_ROM005;
         return SONIX_APP_OK;
     }
     
     *peRomVer = BSP_OID_VER_ROM006;
     return SONIX_APP_OK;
 }
 
 /********************************* END OF FILE ********************************/
 